#include "mpu6050.h"

int read_byte_from_mpu6050(int fd, unsigned char reg, unsigned char *pdata){
	unsigned char wbuf[] = {reg};
	unsigned char rbuf[1] = {0};
	int ret = 0;

	ret = write(fd, wbuf, 1);
	if(ret != 1){
		printf("write reg failed \n");
		return -1;
	}

	ret = read(fd, rbuf, 1);
	if(ret != 1){
		printf("read reg failed \n");
		return -1;
	}

	*pdata = rbuf[0];
	return 0;
}

int read_data_from_mpu6050(int fd, unsigned char reg_H, unsigned char reg_L, unsigned short * pdata){
	
	int ret = 0;
	unsigned char tempchar = 0;
	unsigned short data = 0;
	ret = read_byte_from_mpu6050(fd, reg_L, &tempchar);
	if(ret < 0){
		printf("read reg_L 0x%x failed\n",reg_L);
		return -1;
	}
	data = tempchar;
	ret = read_byte_from_mpu6050(fd, reg_H, &tempchar);
	if(ret < 0){
		printf("read reg_H 0x%x failed\n",reg_H);
		return -1;
	}
	data |= tempchar<<8;
	*pdata = data;
	return 0;

}

int write_to_mpu6050(int fd, unsigned char reg, unsigned char data){
	unsigned char wbuf[] = {reg, data};
	int ret = 0;

	ret = write(fd, wbuf, 2);
	if(ret != 2){
		printf("write reg failed \n");
		return -1;
	}

	return 0;
}

int init_mpu6050(int fd){

	int ret = 0;
	ret = ioctl(fd,I2C_SLAVE,0x68);
	if(ret < 0){
		printf("I2C_SLAVE failed \n");
		return -1; 
	}
	ret = ioctl(fd, I2C_TENBIT,0);
	if(ret < 0){
		printf("I2C_TENBIT failed \n");
		return -1;
	}

	ret = write_to_mpu6050(fd, SMPLRT_DIV, 0x07);
	ret += write_to_mpu6050(fd, CONFIG, 0x06);
	ret += write_to_mpu6050(fd, GYRO_CONFIG, 0xF8);
	ret += write_to_mpu6050(fd, ACCEL_CONFIG, 0x19);
	ret += write_to_mpu6050(fd, PWR_MGMT_1, 0x00);
	if(ret < 0){
		printf("write init reg failed \n");
		return -1;
	}
	return 0;
}
